/*
 * Copyright (c) 2024 University of Padova
 *
 * SPDX-License-Identifier: GPL-2.0-only
 *
 * Author: Matteo Pagin <mattpagg@gmail.com>
 */

#include "ns3/angles.h"
#include "ns3/geographic-positions.h"
#include "ns3/log.h"
#include "ns3/test.h"

NS_LOG_COMPONENT_DEFINE("GeocentricTopocentricConversionTest");

namespace
{

/// Test tolerance with respect to the expected values, in meters
constexpr double TOLERANCE_M = 5;
/// Test tolerance with respect to the expected values, in degrees
constexpr double TOLERANCE_DEG = 5e-3;

/**
 * Reference point 1 for geocentric <--> topocentric conversion.
 * In particular, this points is used for the example conversion of
 * Sec. 4.1.3. of "IOGP Publication 373-7-2 – Geomatics Guidance
 * Note number 7, part 2 – September 2019".
 */
const ns3::Vector REFP_1_COORD{55.0, 5.0, 200.0};

/**
 * Table of <longitude (deg), latitude (deg), altitude (m)> coordinates, obtained as the
 * cartesian product of the longitude values
 * (2.1295495 0.0 30.0 60.0 90.0 120.0 150.0 180.0), latitude values
 * (53.80939444 0.0 20.0 40.0 60.0 80.0) and altitude values
 * (73.0 500.0 1000.0).
 * The first coordinate, i.e., (2.1295495, 53.80939444, 73.0) corresponds to that of the
 * example provided in Sec. 4.1.3. of "IOGP Publication 373-7-2 – Geomatics Guidance
 * Note number 7, part 2 – September 2019".
 * This set of coordinates is, in principle, independent from the reference point.
 */
const std::vector<ns3::Vector> TEST_COORD = {
    {53.809394, 2.129549, 73.000000},     {53.809394, 2.129549, 500.000000},
    {53.809394, 2.129549, 1000.000000},   {0.000000, 2.129549, 73.000000},
    {0.000000, 2.129549, 500.000000},     {0.000000, 2.129549, 1000.000000},
    {20.000000, 2.129549, 73.000000},     {20.000000, 2.129549, 500.000000},
    {20.000000, 2.129549, 1000.000000},   {40.000000, 2.129549, 73.000000},
    {40.000000, 2.129549, 500.000000},    {40.000000, 2.129549, 1000.000000},
    {60.000000, 2.129549, 73.000000},     {60.000000, 2.129549, 500.000000},
    {60.000000, 2.129549, 1000.000000},   {80.000000, 2.129549, 73.000000},
    {80.000000, 2.129549, 500.000000},    {80.000000, 2.129549, 1000.000000},
    {53.809394, 0.000000, 73.000000},     {53.809394, 0.000000, 500.000000},
    {53.809394, 0.000000, 1000.000000},   {0.000000, 0.000000, 73.000000},
    {0.000000, 0.000000, 500.000000},     {0.000000, 0.000000, 1000.000000},
    {20.000000, 0.000000, 73.000000},     {20.000000, 0.000000, 500.000000},
    {20.000000, 0.000000, 1000.000000},   {40.000000, 0.000000, 73.000000},
    {40.000000, 0.000000, 500.000000},    {40.000000, 0.000000, 1000.000000},
    {60.000000, 0.000000, 73.000000},     {60.000000, 0.000000, 500.000000},
    {60.000000, 0.000000, 1000.000000},   {80.000000, 0.000000, 73.000000},
    {80.000000, 0.000000, 500.000000},    {80.000000, 0.000000, 1000.000000},
    {53.809394, 30.000000, 73.000000},    {53.809394, 30.000000, 500.000000},
    {53.809394, 30.000000, 1000.000000},  {0.000000, 30.000000, 73.000000},
    {0.000000, 30.000000, 500.000000},    {0.000000, 30.000000, 1000.000000},
    {20.000000, 30.000000, 73.000000},    {20.000000, 30.000000, 500.000000},
    {20.000000, 30.000000, 1000.000000},  {40.000000, 30.000000, 73.000000},
    {40.000000, 30.000000, 500.000000},   {40.000000, 30.000000, 1000.000000},
    {60.000000, 30.000000, 73.000000},    {60.000000, 30.000000, 500.000000},
    {60.000000, 30.000000, 1000.000000},  {80.000000, 30.000000, 73.000000},
    {80.000000, 30.000000, 500.000000},   {80.000000, 30.000000, 1000.000000},
    {53.809394, 60.000000, 73.000000},    {53.809394, 60.000000, 500.000000},
    {53.809394, 60.000000, 1000.000000},  {0.000000, 60.000000, 73.000000},
    {0.000000, 60.000000, 500.000000},    {0.000000, 60.000000, 1000.000000},
    {20.000000, 60.000000, 73.000000},    {20.000000, 60.000000, 500.000000},
    {20.000000, 60.000000, 1000.000000},  {40.000000, 60.000000, 73.000000},
    {40.000000, 60.000000, 500.000000},   {40.000000, 60.000000, 1000.000000},
    {60.000000, 60.000000, 73.000000},    {60.000000, 60.000000, 500.000000},
    {60.000000, 60.000000, 1000.000000},  {80.000000, 60.000000, 73.000000},
    {80.000000, 60.000000, 500.000000},   {80.000000, 60.000000, 1000.000000},
    {53.809394, 90.000000, 73.000000},    {53.809394, 90.000000, 500.000000},
    {53.809394, 90.000000, 1000.000000},  {0.000000, 90.000000, 73.000000},
    {0.000000, 90.000000, 500.000000},    {0.000000, 90.000000, 1000.000000},
    {20.000000, 90.000000, 73.000000},    {20.000000, 90.000000, 500.000000},
    {20.000000, 90.000000, 1000.000000},  {40.000000, 90.000000, 73.000000},
    {40.000000, 90.000000, 500.000000},   {40.000000, 90.000000, 1000.000000},
    {60.000000, 90.000000, 73.000000},    {60.000000, 90.000000, 500.000000},
    {60.000000, 90.000000, 1000.000000},  {80.000000, 90.000000, 73.000000},
    {80.000000, 90.000000, 500.000000},   {80.000000, 90.000000, 1000.000000},
    {53.809394, 120.000000, 73.000000},   {53.809394, 120.000000, 500.000000},
    {53.809394, 120.000000, 1000.000000}, {0.000000, 120.000000, 73.000000},
    {0.000000, 120.000000, 500.000000},   {0.000000, 120.000000, 1000.000000},
    {20.000000, 120.000000, 73.000000},   {20.000000, 120.000000, 500.000000},
    {20.000000, 120.000000, 1000.000000}, {40.000000, 120.000000, 73.000000},
    {40.000000, 120.000000, 500.000000},  {40.000000, 120.000000, 1000.000000},
    {60.000000, 120.000000, 73.000000},   {60.000000, 120.000000, 500.000000},
    {60.000000, 120.000000, 1000.000000}, {80.000000, 120.000000, 73.000000},
    {80.000000, 120.000000, 500.000000},  {80.000000, 120.000000, 1000.000000},
    {53.809394, 150.000000, 73.000000},   {53.809394, 150.000000, 500.000000},
    {53.809394, 150.000000, 1000.000000}, {0.000000, 150.000000, 73.000000},
    {0.000000, 150.000000, 500.000000},   {0.000000, 150.000000, 1000.000000},
    {20.000000, 150.000000, 73.000000},   {20.000000, 150.000000, 500.000000},
    {20.000000, 150.000000, 1000.000000}, {40.000000, 150.000000, 73.000000},
    {40.000000, 150.000000, 500.000000},  {40.000000, 150.000000, 1000.000000},
    {60.000000, 150.000000, 73.000000},   {60.000000, 150.000000, 500.000000},
    {60.000000, 150.000000, 1000.000000}, {80.000000, 150.000000, 73.000000},
    {80.000000, 150.000000, 500.000000},  {80.000000, 150.000000, 1000.000000},
    {53.809394, 180.000000, 73.000000},   {53.809394, 180.000000, 500.000000},
    {53.809394, 180.000000, 1000.000000}, {0.000000, 180.000000, 73.000000},
    {0.000000, 180.000000, 500.000000},   {0.000000, 180.000000, 1000.000000},
    {20.000000, 180.000000, 73.000000},   {20.000000, 180.000000, 500.000000},
    {20.000000, 180.000000, 1000.000000}, {40.000000, 180.000000, 73.000000},
    {40.000000, 180.000000, 500.000000},  {40.000000, 180.000000, 1000.000000},
    {60.000000, 180.000000, 73.000000},   {60.000000, 180.000000, 500.000000},
    {60.000000, 180.000000, 1000.000000}, {80.000000, 180.000000, 73.000000},
    {80.000000, 180.000000, 500.000000},  {80.000000, 180.000000, 1000.000000}};

/**
 * Table of expected geocentric coordinates <U, V, W> (meters) for spheroid type SPHERE
 * and reference point 1
 */
const std::vector<ns3::Vector> SPHERE_REFP_1 = {
    {-188390.250, -128514.834, -4209.801},     {-188402.876, -128523.447, -3783.074},
    {-188417.661, -128533.533, -3283.395},     {-319049.537, -5212326.981, -2721486.356},
    {-319070.921, -5212676.320, -2721241.747}, {-319095.960, -5213085.382, -2720955.318},
    {-299808.496, -3648142.487, -1156630.460}, {-299828.590, -3648386.992, -1156280.970},
    {-299852.118, -3648673.297, -1155871.733}, {-244406.125, -1643938.169, -220728.246},
    {-244422.506, -1644048.348, -220316.032},  {-244441.687, -1644177.364, -219833.344},
    {-159524.769, 558549.355, -26663.336},     {-159535.460, 558586.790, -26238.115},
    {-159547.980, 558630.625, -25740.197},     {-55402.371, 2693667.583, -597842.821},
    {-55406.084, 2693848.117, -597455.881},    {-55410.432, 2694059.516, -597002.790},
    {-327875.291, -120654.830, -9713.435},     {-327897.266, -120662.917, -9287.077},
    {-327922.997, -120672.386, -8787.830},     {-555275.338, -5199015.620, -2730807.072},
    {-555312.553, -5199364.067, -2730563.087}, {-555356.131, -5199772.084, -2730277.390},
    {-521788.138, -3635633.899, -1165389.067}, {-521823.109, -3635877.566, -1165040.165},
    {-521864.059, -3636162.889, -1164631.614}, {-425365.587, -1633741.074, -227868.329},
    {-425394.096, -1633850.571, -227456.592},  {-425427.478, -1633978.786, -226974.465},
    {-277637.669, 565205.035, -31323.694},     {-277656.277, 565242.916, -30898.785},
    {-277678.066, 565287.274, -30401.233},     {-96422.551, 2695979.077, -599461.346},
    {-96429.013, 2696159.766, -599074.515},    {-96436.580, 2696371.345, -598621.550},
    {1589867.531, 156341.271, -203668.193},    {1589974.086, 156351.749, -203254.835},
    {1590098.859, 156364.019, -202770.808},    {2692530.529, -4729907.065, -3059280.418},
    {2692710.987, -4730224.072, -3059058.447}, {2692922.296, -4730595.274, -3058798.529},
    {2530151.069, -3194816.053, -1474053.046}, {2530320.644, -3195030.175, -1473724.831},
    {2530519.210, -3195280.903, -1473340.505}, {2062598.050, -1274383.073, -479493.510},
    {2062736.288, -1274468.485, -479098.638},  {2062898.161, -1274568.498, -478636.259},
    {1346265.264, 799759.313, -195560.367},    {1346355.493, 799812.914, -195146.465},
    {1346461.148, 799875.679, -194661.803},    {467553.020, 2777438.922, -656500.144},
    {467584.356, 2777625.071, -656117.136},    {467621.049, 2777843.044, -655668.648},
    {3081606.632, 1181688.408, -921623.987},   {3081813.166, 1181767.607, -921258.748},
    {3082055.010, 1181860.345, -920831.067},   {5218875.015, -2993423.733, -4275179.137},
    {5219224.793, -2993624.357, -4275038.659}, {5219634.369, -2993859.280, -4274874.164},
    {4904138.340, -1563055.479, -2616624.101}, {4904467.024, -1563160.237, -2616372.463},
    {4904851.900, -1563282.906, -2616077.805}, {3997890.204, 55840.334, -1410925.968},
    {3998158.150, 55844.077, -1410593.522},    {3998471.903, 55848.459, -1410204.241},
    {2609437.507, 1668000.979, -803509.727},   {2609612.396, 1668112.771, -803136.571},
    {2609817.184, 1668243.676, -802699.620},   {906248.136, 3078976.088, -867638.741},
    {906308.874, 3079182.447, -867269.883},    {906379.996, 3079424.084, -866837.965},
    {3747631.724, 2680645.643, -1971205.143},  {3747882.897, 2680825.304, -1970910.248},
    {3748177.010, 2681035.681, -1970564.938},  {6346826.155, -454854.928, -6052704.150},
    {6347251.530, -454885.413, -6052682.805},  {6347749.628, -454921.110, -6052657.809},
    {5964065.703, 822418.894, -4286951.239},   {5964465.425, 822474.014, -4286811.549},
    {5964933.484, 822538.557, -4286647.978},   {4861950.908, 2000496.861, -2772589.126},
    {4862276.764, 2000630.937, -2772347.942},  {4862658.329, 2000787.936, -2772065.524},
    {3173413.078, 2937285.381, -1692272.233},  {3173625.765, 2937482.243, -1691958.644},
    {3173874.814, 2937712.761, -1691591.443},  {1102114.796, 3519793.935, -1176302.720},
    {1102188.661, 3520029.838, -1175954.550},  {1102275.155, 3520306.070, -1175546.856},
    {3409481.922, 4251568.596, -3071177.236},  {3409710.432, 4251853.543, -3070956.063},
    {3409978.007, 4252187.205, -3070697.079},  {5774150.353, 2205591.887, -7915569.065},
    {5774537.346, 2205739.710, -7915672.572},  {5774990.500, 2205912.804, -7915793.774},
    {5425926.478, 3322421.135, -6037471.653},  {5426290.133, 3322643.809, -6037449.286},
    {5426715.958, 3322904.552, -6037423.096},  {4423255.791, 4038517.360, -4199626.443},
    {4423552.246, 4038788.028, -4199480.900},  {4423899.382, 4039104.970, -4199310.476},
    {2887075.176, 4267508.789, -2623704.691},  {2887268.673, 4267794.805, -2623453.527},
    {2887495.250, 4268129.718, -2623159.425},  {1002670.686, 3981775.676, -1499785.818},
    {1002737.887, 3982042.542, -1499459.328},  {1002816.576, 3982355.031, -1499077.021},
    {2157764.193, 5473529.730, -3926803.634},  {2157908.810, 5473896.575, -3926639.806},
    {2158078.151, 5474326.136, -3926447.971},  {3654295.626, 4275052.137, -9364620.733},
    {3654540.543, 4275338.659, -9364821.357},  {3654827.332, 4275674.164, -9365056.280},
    {3433914.634, 5267077.661, -7399134.812},  {3434144.781, 5267430.669, -7399203.706},
    {3434414.274, 5267844.028, -7399284.378},  {2799352.858, 5623815.885, -5309664.420},
    {2799540.475, 5624192.802, -5309593.275},  {2799760.168, 5624634.158, -5309509.966},
    {1827147.813, 5302238.914, -3348230.524},  {1827270.272, 5302594.279, -3348027.920},
    {1827413.666, 5303010.398, -3347790.678},  {634561.776, 4341133.678, -1751411.000},
    {634604.306, 4341424.628, -1751101.374},   {634654.106, 4341765.319, -1750738.814},
    {327875.291, 6019105.546, -4308819.933},   {327897.266, 6019508.957, -4308681.709},
    {327922.997, 6019981.334, -4308519.854},   {555275.338, 5199015.620, -10011586.928},
    {555312.553, 5199364.067, -10011830.913},  {555356.131, 5199772.084, -10012116.610},
    {521788.138, 6135319.327, -8007084.172},   {521823.109, 6135730.527, -8007193.812},
    {521864.059, 6136212.025, -8007322.196},   {425365.587, 6331612.976, -5805269.279},
    {425394.096, 6332037.331, -5805231.350},   {425427.478, 6332534.235, -5805186.936},
    {277637.669, 5764220.655, -3671713.622},   {277656.277, 5764606.983, -3671532.698},
    {277678.066, 5765059.358, -3671320.843},   {96422.551, 4501578.253, -1863755.500},
    {96429.013, 4501879.956, -1863453.404},    {96436.580, 4502233.239, -1863099.661}};

/**
 * Table of expected geocentric coordinates <U, V, W> (meters) for spheroid type GRS80
 * and reference point 1
 */
const std::vector<ns3::Vector> GRS80_REFP_1 = {
    {-189013.904, -128642.088, -4220.173},     {-189026.530, -128650.701, -3793.446},
    {-189041.315, -128660.787, -3293.767},     {-319407.093, -5198061.834, -2710194.751},
    {-319428.477, -5198411.173, -2709950.141}, {-319453.515, -5198820.235, -2709663.713},
    {-300262.077, -3641934.701, -1153507.066}, {-300282.171, -3642179.206, -1153157.577},
    {-300305.699, -3642465.511, -1152748.339}, {-245019.116, -1643718.675, -220614.182},
    {-245035.496, -1643828.854, -220201.967},  {-245054.677, -1643957.870, -219719.280},
    {-160105.977, 559428.176, -26712.769},     {-160116.668, 559465.611, -26287.548},
    {-160129.188, 559509.446, -25789.630},     {-55645.391, 2701392.816, -599873.698},
    {-55649.104, 2701573.351, -599486.758},    {-55653.452, 2701784.749, -599033.667},
    {-328960.701, -120756.064, -9742.026},     {-328982.676, -120764.150, -9315.669},
    {-329008.407, -120773.619, -8816.421},     {-555897.630, -5184735.555, -2719525.912},
    {-555934.845, -5185084.002, -2719281.927}, {-555978.423, -5185492.019, -2718996.230},
    {-522577.552, -3629407.189, -1162278.924}, {-522612.523, -3629650.855, -1161930.022},
    {-522653.473, -3629936.179, -1161521.471}, {-426432.439, -1633496.005, -227772.172},
    {-426460.948, -1633605.501, -227360.436},  {-426494.330, -1633733.717, -226878.309},
    {-278649.206, 566108.106, -31390.106},     {-278667.814, 566145.987, -30965.197},
    {-278689.603, 566190.344, -30467.646},     {-96845.504, 2703714.449, -601499.323},
    {-96851.967, 2703895.138, -601112.492},    {-96859.534, 2704106.718, -600659.527},
    {1595130.685, 157157.015, -204338.859},    {1595237.241, 157167.493, -203925.501},
    {1595362.013, 157179.763, -203441.475},    {2695548.023, -4715101.275, -3048367.375},
    {2695728.481, -4715418.281, -3048145.405}, {2695939.790, -4715789.483, -3047885.487},
    {2533978.941, -3187922.428, -1471409.882}, {2534148.516, -3188136.550, -1471081.667},
    {2534347.082, -3188387.278, -1470697.340}, {2067771.215, -1273236.705, -480028.450},
    {2067909.453, -1273322.116, -479633.578},  {2068071.326, -1273422.129, -479171.199},
    {1351170.209, 801516.952, -196225.155},    {1351260.438, 801570.553, -195811.253},
    {1351366.092, 801633.318, -195326.590},    {469603.923, 2785531.615, -658788.320},
    {469635.260, 2785717.764, -658405.311},    {469671.953, 2785935.737, -657956.823},
    {3091808.092, 1185898.498, -924671.400},   {3092014.626, 1185977.696, -924306.160},
    {3092256.470, 1186070.435, -923878.479},   {5224723.760, -2976671.881, -4265628.741},
    {5225073.538, -2976872.506, -4265488.263}, {5225483.114, -2977107.429, -4265323.768},
    {4911557.823, -1553693.159, -2615709.535}, {4911886.507, -1553797.918, -2615457.897},
    {4912271.382, -1553920.586, -2615163.239}, {4007917.241, 60323.012, -1413797.017},
    {4008185.186, 60326.755, -1413464.571},    {4008498.940, 60331.137, -1413075.290},
    {2618944.657, 1672921.945, -806389.500},   {2619119.546, 1673033.737, -806016.344},
    {2619324.334, 1673164.641, -805579.393},   {910223.359, 3088391.463, -870853.068},
    {910284.098, 3088597.821, -870484.210},    {910355.220, 3088839.459, -870052.292},
    {3760038.018, 2689817.934, -1977727.126},  {3760289.190, 2689997.596, -1977432.231},
    {3760583.303, 2690207.972, -1977086.921},  {6353938.985, -435258.126, -6045145.811},
    {6354364.360, -435288.611, -6045124.465},  {6354862.458, -435324.308, -6045099.469},
    {5973088.753, 835390.203, -4288563.715},   {5973488.475, 835445.323, -4288424.025},
    {5973956.533, 835509.867, -4288260.454},   {4874145.079, 2009856.896, -2778875.338},
    {4874470.935, 2009990.973, -2778634.153},  {4874852.500, 2010147.972, -2778351.735},
    {3184975.000, 2946830.821, -1698390.098},  {3185187.687, 2947027.683, -1698076.509},
    {3185436.736, 2947258.201, -1697709.308},  {1106949.181, 3531142.941, -1180870.991},
    {1107023.047, 3531378.844, -1180522.820},  {1107109.540, 3531655.076, -1180115.126},
    {3420768.793, 4265941.326, -3081340.607},  {3420997.302, 4266226.274, -3081119.434},
    {3421264.877, 4266559.936, -3080860.449},  {5780621.390, 2228170.228, -7910098.421},
    {5781008.384, 2228318.050, -7910201.928},  {5781461.538, 2228491.145, -7910323.130},
    {5434135.375, 3339174.703, -6041732.496},  {5434499.030, 3339397.377, -6041710.129},
    {5434924.855, 3339658.120, -6041683.938},  {4434349.680, 4052988.918, -4209491.781},
    {4434646.134, 4053259.586, -4209346.239},  {4434993.270, 4053576.528, -4209175.814},
    {2897593.863, 4281900.727, -2633216.110},  {2897787.360, 4282186.743, -2632964.947},
    {2898013.937, 4282521.656, -2632670.844},  {1007068.864, 3995151.148, -1505773.035},
    {1007136.064, 3995418.013, -1505446.544},  {1007214.754, 3995730.502, -1505064.237},
    {2164907.333, 5491947.684, -3939799.500},  {2165051.950, 5492314.529, -3939635.673},
    {2165221.290, 5492744.090, -3939443.837},  {3658390.962, 4299949.703, -9360774.028},
    {3658635.879, 4300236.225, -9360974.652},  {3658922.667, 4300571.730, -9361209.575},
    {3439109.812, 5286773.305, -7405455.718},  {3439339.959, 5287126.313, -7405524.612},
    {3439609.452, 5287539.672, -7405605.284},  {2806373.864, 5642263.501, -5322313.825},
    {2806561.482, 5642640.419, -5322242.679},  {2806781.174, 5643081.775, -5322159.370},
    {1833804.791, 5320400.758, -3360381.660},  {1833927.250, 5320756.123, -3360179.056},
    {1834070.644, 5321172.242, -3359941.814},  {637345.258, 4356085.459, -1758501.961},
    {637387.787, 4356376.410, -1758192.335},   {637437.587, 4356717.101, -1757829.775},
    {328960.701, 6039329.594, -4323080.440},   {328982.676, 6039733.004, -4322942.215},
    {329008.407, 6040205.382, -4322780.361},   {555897.630, 5224948.663, -10008465.272},
    {555934.845, 5225297.110, -10008709.257},  {555978.423, 5225705.128, -10008994.954},
    {522577.552, 6156328.536, -8014324.846},   {522612.523, 6156739.736, -8014434.486},
    {522653.473, 6157221.234, -8014562.870},   {426432.439, 6351835.806, -5819161.701},
    {426460.948, 6352260.161, -5819123.772},   {426494.330, 6352757.065, -5819079.358},
    {278649.206, 5784065.670, -3685043.327},   {278667.814, 5784451.998, -3684862.403},
    {278689.603, 5784904.373, -3684650.548},   {96845.504, 4517233.818, -1871339.256},
    {96851.967, 4517535.522, -1871037.160},    {96859.534, 4517888.805, -1870683.417}};

/**
 * Table of expected geocentric coordinates <U, V, W> (meters) for spheroid type WGS84
 * and reference point 1
 */
const std::vector<ns3::Vector> WGS_84_REFP_1 = {
    {-189013.904, -128642.088, -4220.173},     {-189026.530, -128650.701, -3793.446},
    {-189041.315, -128660.787, -3293.767},     {-319407.093, -5198061.834, -2710194.751},
    {-319428.477, -5198411.173, -2709950.141}, {-319453.515, -5198820.235, -2709663.713},
    {-300262.077, -3641934.701, -1153507.066}, {-300282.171, -3642179.206, -1153157.577},
    {-300305.699, -3642465.511, -1152748.339}, {-245019.116, -1643718.675, -220614.182},
    {-245035.496, -1643828.854, -220201.967},  {-245054.677, -1643957.870, -219719.280},
    {-160105.977, 559428.176, -26712.769},     {-160116.668, 559465.611, -26287.548},
    {-160129.188, 559509.446, -25789.630},     {-55645.391, 2701392.816, -599873.698},
    {-55649.104, 2701573.351, -599486.758},    {-55653.452, 2701784.749, -599033.667},
    {-328960.701, -120756.064, -9742.026},     {-328982.676, -120764.150, -9315.669},
    {-329008.407, -120773.619, -8816.421},     {-555897.630, -5184735.555, -2719525.912},
    {-555934.845, -5185084.002, -2719281.927}, {-555978.423, -5185492.019, -2718996.230},
    {-522577.552, -3629407.189, -1162278.924}, {-522612.523, -3629650.855, -1161930.022},
    {-522653.473, -3629936.179, -1161521.471}, {-426432.439, -1633496.005, -227772.172},
    {-426460.948, -1633605.501, -227360.436},  {-426494.330, -1633733.717, -226878.309},
    {-278649.206, 566108.106, -31390.106},     {-278667.814, 566145.987, -30965.197},
    {-278689.603, 566190.344, -30467.646},     {-96845.504, 2703714.449, -601499.323},
    {-96851.967, 2703895.138, -601112.492},    {-96859.534, 2704106.718, -600659.527},
    {1595130.685, 157157.015, -204338.859},    {1595237.241, 157167.493, -203925.501},
    {1595362.013, 157179.763, -203441.475},    {2695548.023, -4715101.275, -3048367.375},
    {2695728.481, -4715418.282, -3048145.405}, {2695939.790, -4715789.483, -3047885.487},
    {2533978.941, -3187922.428, -1471409.882}, {2534148.516, -3188136.550, -1471081.667},
    {2534347.082, -3188387.278, -1470697.340}, {2067771.214, -1273236.705, -480028.450},
    {2067909.453, -1273322.116, -479633.578},  {2068071.326, -1273422.129, -479171.199},
    {1351170.209, 801516.952, -196225.155},    {1351260.438, 801570.553, -195811.253},
    {1351366.092, 801633.318, -195326.590},    {469603.923, 2785531.615, -658788.320},
    {469635.260, 2785717.764, -658405.311},    {469671.953, 2785935.737, -657956.823},
    {3091808.092, 1185898.498, -924671.400},   {3092014.626, 1185977.696, -924306.160},
    {3092256.470, 1186070.435, -923878.479},   {5224723.760, -2976671.881, -4265628.741},
    {5225073.538, -2976872.506, -4265488.263}, {5225483.114, -2977107.429, -4265323.768},
    {4911557.823, -1553693.160, -2615709.535}, {4911886.507, -1553797.918, -2615457.897},
    {4912271.382, -1553920.586, -2615163.239}, {4007917.241, 60323.012, -1413797.016},
    {4008185.186, 60326.755, -1413464.571},    {4008498.940, 60331.137, -1413075.290},
    {2618944.657, 1672921.945, -806389.500},   {2619119.546, 1673033.737, -806016.344},
    {2619324.334, 1673164.641, -805579.393},   {910223.359, 3088391.463, -870853.068},
    {910284.098, 3088597.821, -870484.210},    {910355.220, 3088839.459, -870052.292},
    {3760038.017, 2689817.934, -1977727.126},  {3760289.190, 2689997.596, -1977432.231},
    {3760583.303, 2690207.972, -1977086.921},  {6353938.985, -435258.126, -6045145.811},
    {6354364.360, -435288.611, -6045124.465},  {6354862.458, -435324.308, -6045099.470},
    {5973088.753, 835390.203, -4288563.715},   {5973488.475, 835445.323, -4288424.025},
    {5973956.533, 835509.867, -4288260.454},   {4874145.079, 2009856.896, -2778875.338},
    {4874470.935, 2009990.973, -2778634.153},  {4874852.500, 2010147.972, -2778351.735},
    {3184975.000, 2946830.821, -1698390.098},  {3185187.687, 2947027.683, -1698076.509},
    {3185436.736, 2947258.201, -1697709.308},  {1106949.181, 3531142.941, -1180870.990},
    {1107023.047, 3531378.843, -1180522.820},  {1107109.540, 3531655.076, -1180115.126},
    {3420768.793, 4265941.326, -3081340.607},  {3420997.302, 4266226.274, -3081119.434},
    {3421264.877, 4266559.936, -3080860.449},  {5780621.390, 2228170.228, -7910098.421},
    {5781008.384, 2228318.050, -7910201.928},  {5781461.538, 2228491.145, -7910323.130},
    {5434135.375, 3339174.703, -6041732.496},  {5434499.030, 3339397.377, -6041710.129},
    {5434924.855, 3339658.120, -6041683.938},  {4434349.679, 4052988.918, -4209491.781},
    {4434646.134, 4053259.586, -4209346.239},  {4434993.270, 4053576.528, -4209175.814},
    {2897593.863, 4281900.727, -2633216.110},  {2897787.360, 4282186.743, -2632964.947},
    {2898013.937, 4282521.656, -2632670.844},  {1007068.864, 3995151.148, -1505773.035},
    {1007136.064, 3995418.013, -1505446.544},  {1007214.754, 3995730.502, -1505064.237},
    {2164907.333, 5491947.684, -3939799.500},  {2165051.950, 5492314.529, -3939635.673},
    {2165221.290, 5492744.090, -3939443.837},  {3658390.962, 4299949.703, -9360774.028},
    {3658635.879, 4300236.225, -9360974.652},  {3658922.667, 4300571.730, -9361209.575},
    {3439109.812, 5286773.305, -7405455.718},  {3439339.959, 5287126.313, -7405524.612},
    {3439609.452, 5287539.672, -7405605.284},  {2806373.864, 5642263.501, -5322313.825},
    {2806561.482, 5642640.419, -5322242.679},  {2806781.174, 5643081.775, -5322159.370},
    {1833804.791, 5320400.758, -3360381.660},  {1833927.250, 5320756.123, -3360179.056},
    {1834070.644, 5321172.242, -3359941.814},  {637345.258, 4356085.459, -1758501.961},
    {637387.787, 4356376.410, -1758192.335},   {637437.587, 4356717.101, -1757829.775},
    {328960.701, 6039329.593, -4323080.439},   {328982.676, 6039733.004, -4322942.215},
    {329008.407, 6040205.382, -4322780.361},   {555897.630, 5224948.663, -10008465.272},
    {555934.845, 5225297.110, -10008709.257},  {555978.423, 5225705.128, -10008994.954},
    {522577.552, 6156328.536, -8014324.846},   {522612.523, 6156739.736, -8014434.486},
    {522653.473, 6157221.234, -8014562.870},   {426432.439, 6351835.806, -5819161.701},
    {426460.948, 6352260.161, -5819123.772},   {426494.330, 6352757.065, -5819079.358},
    {278649.206, 5784065.670, -3685043.327},   {278667.814, 5784451.998, -3684862.403},
    {278689.603, 5784904.373, -3684650.548},   {96845.504, 4517233.818, -1871339.256},
    {96851.967, 4517535.522, -1871037.160},    {96859.534, 4517888.805, -1870683.417}};
} // namespace

using namespace ns3;

/**
 * @ingroup mobility-test
 *
 * @brief Geographic to/from Topocentric Test Case
 *
 * This test verifies the accuracy of the GeographicToTopocentricCoordinates and
 * TopocentricToGeographicCoordinates methods in the GeographicPositions class,
 * which convert geocentric (topocentric) coordinates to topocentric (geocentric)
 * coordinates, respectively. In "IOGP Publication 373-7-2 – Geomatics Guidance Note number 7, part
 * 2 – September 2019", the two operations are also referred to as EPSG Dataset coordinate operation
 * method code 9837.
 *
 * To this end, the values generated using the above methods are compared with the coordinates
 * obtained using PROJ coordinate conversion utilities
 * (https://proj.org/en/latest/operations/conversions/topocentric.html), for both sphere, GRS80 and
 * WGS84 ellipsoid models. For instance, the coordinate (long, lat, alt) has been first
 * converted to the topocentric coordinate system with reference point in (LONG_0, LAT_0, ALT_0)
 * with the command echo $long $lat $alt | cct -d 3 +proj=pipeline +ellps=$ellps +step +proj=cart
 * +step +proj=topocentric +lon_0=$LONG_0 +lat_0=$LAT_0 +h_0=$ALT_0. Then, the value obtained via
 * PROJ is used to check both the forward (geo -> topo) and reverse (topo -> geo) conversion
 */
class GeoToAndFromTopocentricTestCase : public TestCase
{
  public:
    /**
     * @brief Constructor
     *
     * @param posToConvert a vector of positions to convert in geocentric coordinates, i.e.,
     * represented as a vector (latitude (deg), longitude (deg), altitude (m))
     * @param refPoint  the reference point, in geocentric coordinates, i.e., represented as a
     * vector (latitude (deg), longitude (deg), altitude (m))
     * @param expectedTopoCoord a vector of the corresponding expected topocentric coordinates,
     * i.e., represented as (U (m), V (m), W (m)) with respect to the provided reference point
     * @param sphereType spheroid type
     */
    GeoToAndFromTopocentricTestCase(const std::vector<Vector> posToConvert,
                                    Vector refPoint,
                                    const std::vector<Vector> expectedTopoCoord,
                                    GeographicPositions::EarthSpheroidType sphereType);

  private:
    void DoRun() override;

    /**
     * @brief Test the forward and backward conversion for geocentric to topocentric coordinates
     * for a given reference point
     *
     * @param posToConvert the position to convert, in geocentric coordinates, i.e., represented as
     * a vector (latitude (deg), longitude (deg), altitude (m))
     * @param refPoint  the reference point, in geocentric coordinates, i.e., represented as a
     * vector (latitude (deg), longitude (deg), altitude (m))
     * @param expectedTopoCoord the expected corresponding topocentric coordinates, i.e.,
     * represented as (U (m), V (m), W (m)) with respect to the provided reference point
     * @param sphereType spheroid type
     */
    void TestReferencePoint(Vector posToConvert,
                            Vector refPoint,
                            Vector expectedTopoCoord,
                            GeographicPositions::EarthSpheroidType sphereType);

    /**
     * Manually check the test condition, i.e. equality between the expected
     * and the obtained values, up to a given tolerance.
     * @param actual the obtained test value
     * @param limit the expected test value
     * @param tol the tolerance error
     * @param coord the string to display to describe the value
     * @return whether the provided values satisfy the equality condition
     */
    bool CheckTestPass(double actual, double limit, double tol, std::string coord);

    /// The position to convert, in geocentric coordinates, i.e.,
    /// represented as a vector (latitude (deg), longitude (deg)///< altitude (m))
    const std::vector<Vector> m_posToConvertGeoCoord;

    /// The reference point, in geocentric coordinates, i.e., represented
    /// as a vector (latitude (deg), longitude (deg), altitude (m))
    Vector m_refPointGeoCoord;

    /// The expected corresponding topocentric coordinates, i.e.,
    /// represented as (U (m), V (m), W (m)) with respect to the
    /// provided reference point
    const std::vector<Vector> m_expectedTopoCoord;

    GeographicPositions::EarthSpheroidType m_sphereType; ///< spheroid type
};

GeoToAndFromTopocentricTestCase::GeoToAndFromTopocentricTestCase(
    const std::vector<Vector> posToConvert,
    Vector refPoint,
    const std::vector<Vector> expectedTopoCoord,
    GeographicPositions::EarthSpheroidType sphereType)
    : TestCase("Creating GeoToAndFromTopocentricTestCase"),
      m_posToConvertGeoCoord(posToConvert),
      m_refPointGeoCoord(refPoint),
      m_expectedTopoCoord(expectedTopoCoord),
      m_sphereType(sphereType)
{
}

void
GeoToAndFromTopocentricTestCase::TestReferencePoint(
    Vector posToConvert,
    Vector refPoint,
    Vector expectedTopoCoord,
    GeographicPositions::EarthSpheroidType sphereType)
{
    // Check forward conversion from geocentric to topocentric
    Vector topoForwardConv =
        GeographicPositions::GeographicToTopocentricCoordinates(posToConvert, refPoint, sphereType);

    bool passGeoToTopo = (CheckTestPass(topoForwardConv.x, expectedTopoCoord.x, TOLERANCE_M, "U") &&
                          CheckTestPass(topoForwardConv.y, expectedTopoCoord.y, TOLERANCE_M, "V") &&
                          CheckTestPass(topoForwardConv.z, expectedTopoCoord.z, TOLERANCE_M, "W"));

    // Check reverse conversion from topocentric to geocentric
    Vector geoReverseConv = GeographicPositions::TopocentricToGeographicCoordinates(topoForwardConv,
                                                                                    refPoint,
                                                                                    sphereType);
    geoReverseConv.x = WrapTo360(geoReverseConv.x);
    geoReverseConv.y = WrapTo360(geoReverseConv.y);

    bool passTopoToGeo = (CheckTestPass(geoReverseConv.x, posToConvert.x, TOLERANCE_DEG, "LAT") &&
                          CheckTestPass(geoReverseConv.y, posToConvert.y, TOLERANCE_DEG, "LONG") &&
                          CheckTestPass(geoReverseConv.z, posToConvert.z, TOLERANCE_M, "ALT"));

    std::string geoToTopoMsg = passGeoToTopo ? "PASS" : "FAIL";
    std::string topoToGeoMsg = passTopoToGeo ? "PASS" : "FAIL";

    std::cout << geoToTopoMsg << " geocentric->topocentric for spheroid " << sphereType << " pos "
              << posToConvert << " reference point " << refPoint << " expected coord "
              << expectedTopoCoord << " actual " << topoForwardConv << std::endl;
    std::cout << topoToGeoMsg << " topocentric->geocentric for spheroid " << sphereType << " pos "
              << topoForwardConv << " reference point " << refPoint << " expected coord "
              << posToConvert << " actual " << geoReverseConv << std::endl;
}

void
GeoToAndFromTopocentricTestCase::DoRun()
{
    // Loop through test coordinates
    for (unsigned int i = 0; i < m_posToConvertGeoCoord.size(); i++)
    {
        TestReferencePoint(m_posToConvertGeoCoord.at(i),
                           m_refPointGeoCoord,
                           m_expectedTopoCoord.at(i),
                           m_sphereType);
    }
}

bool
GeoToAndFromTopocentricTestCase::CheckTestPass(double actual,
                                               double limit,
                                               double tol,
                                               std::string coord)
{
    bool pass = ((actual) < (limit) + (tol) && (actual) > (limit) - (tol));

    NS_TEST_EXPECT_MSG_EQ_TOL(actual,
                              limit,
                              tol,
                              "Mismatch between expected " + coord + " and actual coordinates");

    return pass;
}

/**
 * @ingroup mobility-test
 *
 * @brief Geographic cartesian <--> Topocentric conversion test
 */
class GeoToAndFromTopocentricTestSuite : public TestSuite
{
  public:
    GeoToAndFromTopocentricTestSuite();
};

GeoToAndFromTopocentricTestSuite::GeoToAndFromTopocentricTestSuite()
    : TestSuite("geographic-topocentric-test", Type::UNIT)
{
    NS_LOG_INFO("creating GeoToAndFromTopocentricTestSuite");

    // Test conversion for sphere ellipsoid model
    AddTestCase(new GeoToAndFromTopocentricTestCase(TEST_COORD,    // pos
                                                    REFP_1_COORD,  // ref point
                                                    SPHERE_REFP_1, // expected topo coord
                                                    GeographicPositions::SPHERE),
                TestCase::Duration::QUICK);

    // Test conversion for GRS80 ellipsoid model
    AddTestCase(new GeoToAndFromTopocentricTestCase(TEST_COORD,   // pos
                                                    REFP_1_COORD, // ref point
                                                    GRS80_REFP_1, // expected topo coord
                                                    GeographicPositions::GRS80),
                TestCase::Duration::QUICK);

    // Test conversion for WGS84 ellipsoid model
    AddTestCase(new GeoToAndFromTopocentricTestCase(TEST_COORD,    // pos
                                                    REFP_1_COORD,  // ref point
                                                    WGS_84_REFP_1, // expected topo coord
                                                    GeographicPositions::WGS84),
                TestCase::Duration::QUICK);
}

/**
 * @ingroup mobility-test
 * Static variable for test initialization
 */
static GeoToAndFromTopocentricTestSuite g_GeoToAndFromTopocentricTestSuite;
